import robomaster
from robomaster import robot
from robomaster import config
import time 
from cv2 import cv2
import detection 


# def capphoto(filename):

#     tl_camera.start_video_stream(display=False)
#     time.sleep(1)
#     tl_camera.set_fps("high")
#     tl_camera.set_resolution("high")
#     tl_camera.set_bitrate(6)
#     for i in range(0, 100):
#         img = tl_camera.read_cv2_image()
#         cv2.imshow("Drone", img)
#         cv2.waitKey(1)
#         cv2.imwrite('./photo/neg/'+filename+'.jpg',img)
#     cv2.destroyAllWindows()
#     tl_camera.stop_video_stream()
class TT():
        
    def __init__(self):

        self.tl_drone=robot.Drone()
        self.tl_drone.initialize()
        self.tl_drone.config_sta(ssid="robomaster",password="robo2021")
        print('TTok')


    def main(self):

      
        time.sleep(2)
        tl_flight = self.tl_drone.flight
        tl_camera = self.tl_drone.camera
        tl_led=self.tl_drone.led
        print('TTok2')

        #设置显示图案
        #tl_led.set_mled_sc()
        str_1='000rr00000rrr0000rrrr000rr0rr000000rr000000rr000000rr000000rr000'
        str_2='0rrrrrr0000000r0000000r00rrrrrr00r0000000r0000000r0000000rrrrrr0'
        str_3='0rrrrrr0000000r0000000r00rrrrrr0000000r0000000r0000000r00rrrrrr0'
        str_4='0000r000000rr00000r0r0000r00r000rrrrrrrr0000r0000000r0000000r000'
        str_down='000rr000000rr000000rr000000rr000r00rr00r0r0rr0r000rrrr00000rr000'
        str_wifi='000000r0000000r00000r0r00000r0r000r0r0r0r0r0r0r0r0r0r0r0r0r0r0r0'
        str_list=[str_1,str_2,str_3,str_4,str_down,str_wifi]

        
        # 起飞
        tl_flight.takeoff().wait_for_completed()
    
        # 飞到3号核电站上方
        tl_flight.go(x=150, y=-50, z=-30, speed=30).wait_for_completed()
        print('到达3号核电站上方')

        #拍照
        tl_camera = self.tl_drone.camera
        tl_camera.start_video_stream(display=False)
        time.sleep(1)
        tl_camera.set_fps("high")
        tl_camera.set_resolution("high")
        tl_camera.set_bitrate(6)
        for i in range(0, 100):
            img = tl_camera.read_cv2_image()
            cv2.imshow("Drone", img)
            cv2.waitKey(1)
            cv2.imwrite('./photo/neg/ep2target.jpg',img)
        cv2.destroyAllWindows()
        tl_camera.stop_video_stream()
        #对比图片
        tar34=detection.juicephoto(3)
        print(tar34)
        print('完成3号核电站拍照对比')
        if (tar34<30):
            EProbot2_target=4
            tl_led.set_mled_graph(str_list[3])
        else:
            EProbot2_target=3
            tl_led.set_mled_graph(str_list[2])
        print('2号工程兵的目标：{}号核电站'.format(EProbot2_target))
    
        
        # 飞到2号核电站上方
        #tl_flight.go(x=0, y=60, z=0, speed=30).wait_for_completed()
        tl_flight.left(distance=85).wait_for_completed()
        print('到达2号核电站上方')
        tl_camera = self.tl_drone.camera
        tl_camera.start_video_stream(display=False)
        time.sleep(1)
        tl_camera.set_fps("high")
        tl_camera.set_resolution("high")
        tl_camera.set_bitrate(6)
        for i in range(0, 100):
            img = tl_camera.read_cv2_image()
            cv2.imshow("Drone", img)
            cv2.waitKey(1)
            cv2.imwrite('./photo/neg/ep1target.jpg',img)
        cv2.destroyAllWindows()
        tl_camera.stop_video_stream()
        tar12=detection.juicephoto(2)
        print(tar12)
        print('完成2号核电站拍照对比')
        if (tar12<30):
            EProbot1_target=1
            tl_led.set_mled_graph(str_list[0])
        else:
            EProbot1_target=2
            tl_led.set_mled_graph(str_list[1])
        print('1号工程兵的目标：{}号核电站'.format(EProbot1_target))

        #移动到H区域
        tl_flight.right(distance=40).wait_for_completed()
        #闪烁显示向下箭头
        i=1
        while i<4:
            tl_led.set_mled_graph(str_list[-2])
            time.sleep(1)
            tl_led.set_mled_sc()
            time.sleep(1)
            i+=1
        #降落
        tl_flight.land().wait_for_completed()
        #显示连接信号
        tl_led.set_mled_graph(str_list[-1])

        self.tl_drone.close()

        return EProbot1_target,EProbot2_target

